#include <opencv2/opencv.hpp>
#include <opencv2/opencv.hpp>
#include <iostream>
#include <string.h>
#include <stdlib.h>
#include <time.h>

using namespace cv;
using namespace std;

#define red 0
#define blue 1

void drawRect(Mat src,RotatedRect temp)
{
    // 绘制旋转矩阵
    Point2f temp_[4];
    temp.points(temp_);
    for(int i = 0; i < 4; i++)
    {
        line(src, temp_[i], temp_[(i+1) % 4], Scalar(255,12,12), 2, 8);
    }
    return;
}

double getDistance(Point p1, Point p2)
{
    double distance = pow((p1.x - p2.x), 2) + pow((p1.y - p2.y), 2);
    distance = sqrt(distance);
    return distance;
}

Point getRotateRectCenter(RotatedRect temp1)
{
    Point2f edge_points[4];
    temp1.points(edge_points);
    return Point((edge_points[0].x + edge_points[2].x)/2,(edge_points[0].y + edge_points[2].y)/2);
}


string convInttosring(int a)
{
    char s[40];
    string s_;
    sprintf(s,"%d",a);
    s_ = s;
    return s_;
}

vector<Point> detectpoints(Mat sample,RotatedRect temp1, RotatedRect temp2)
{
    Mat black(sample.size(),CV_8UC1,Scalar(0,0,0));
    drawRect(black,temp1);
    drawRect(black,temp2);
    vector<Point> result;
    //设置角点检测参数
	std::vector<cv::Point2f> corners;
	int max_corners = 20;
	double quality_level = 0.01;
	double min_distance = 3.0;
	int block_size = 3;
	bool use_harris = false;
	double k = 0.04;
    cv::goodFeaturesToTrack(black, 
							corners, 
							max_corners, 
							quality_level, 
							min_distance, 
							cv::Mat(), 
							block_size, 
							use_harris, 
							k);
    for(int i = 0; i < corners.size(); i++)
    {
        circle(black, corners[i], 3, Scalar(0,255,255), -1);
    }

    Point left_up,left_down,right_up,right_down;
    int minIndex = 0;
    int thenIndex = 1;
    int time = 0;
    // 查找最上角点，再分为左上、右上
    for(int i = 0; i < corners.size(); i++)
    {
        if(corners[i].y < corners[minIndex].y)
        {
            minIndex = i;
        }
    }
    // 这一步的目的在于如果thenIndex初始化的时候本身就是一个很靠上面的点，然而
    // 本身可以跳过abs(corners[i].x - corners[minIndex].x) < 50这个检测，那么
    // 就有可能导致两个点在同一边的情况
    while(time < 100){
        if(abs(corners[thenIndex].x - corners[minIndex].x) >50) break;
        else
        {
            thenIndex = rand() % (corners.size());
        }
        time++;
    }
    for(int i = 0; i < corners.size(); i++)
    {
        
        if(i == minIndex) continue;
        if(abs(corners[i].x - corners[minIndex].x) < 50) continue;
        // cout<<abs(corners[i].y - corners[minIndex].y)<<endl;
        if(corners[i].y < corners[thenIndex].y)
        {
            thenIndex = i;
        }
    }
    // cout<<abs(corners[thenIndex].y - corners[minIndex].y)<<endl;
    if(corners[minIndex].x > corners[thenIndex].x)
    {
        right_up = corners[minIndex];
        left_up = corners[thenIndex];
    }
    else
    {
        left_up = corners[minIndex];
        right_up = corners[thenIndex];
    }


    int maxIndex = 0;
    int evenIndex = 1;
    //查找最下角点，再分为左下、右下
    for(int i = 0; i < corners.size(); i++)
    {
        if(corners[i].y > corners[maxIndex].y)
        {
            maxIndex = i;
        }
    }
    time = 0;
    while(time < 100){
        if(abs(corners[evenIndex].x - corners[maxIndex].x) >50) break;
        else
        {
            evenIndex = rand() % (corners.size());
        }
        time++;
    }
    for(int i = 0; i < corners.size(); i++)
    {
        if(i == maxIndex) continue;
        if(abs(corners[i].x - corners[maxIndex].x) < 50) continue;
        if(corners[i].y > corners[evenIndex].y)
        {
            evenIndex = i;
        }
    }
    if(corners[maxIndex].x > corners[evenIndex].x)
    {
        right_down = corners[maxIndex];
        left_down = corners[evenIndex];
    }
    else
    {
        left_down = corners[maxIndex];
        right_down = corners[evenIndex];
    }
    result.push_back(left_up);
    result.push_back(right_up);
    result.push_back(right_down);
    result.push_back(left_down);
    return result;
}

// vector<Point> getall(vector<RotatedRect> rotatedRects,int i,int j)
// {
//     //其中i代表左灯条，j代表右灯条
//     Point temp1[4];
//     rotatedRects[i].points(temp1);
//     Point temp2[4];
//     rotatedRects[j].points(temp2);
//     Point 
//     int length = 8;
//     for(int i = 0; i < length; i++)
//     {
//         if(i < 4)
//         {
//             temp3[i] = temp1[i]
//         }

//     }
//     temp1.insert(temp1.end(),temp2.begin(),temp2.end());
//     cout<<temp1.size()<<endl;
//     return temp1;
// }


int main()
{
    srand((unsigned)time(NULL));
    VideoCapture cap;
    cap.open("../../video/armor(little).mp4");
    Mat src;     //用来暂存图片某一帧
    Mat midImage; //中间图片
    Mat binImage; //掩模图片
    
    vector<vector<Point>> contours;
    vector<vector<Point>> contours_;                        //经过初步筛选之后的轮廓
    vector<Vec4i> hierarchy;
    vector<RotatedRect> rotate_Rect_Group;
    vector<Rect> rect_Group;
    vector<Mat> channels;

    //两灯条中心点
    Point center1;
    Point center2;
    vector<Point> interstPoints; //匹配的燈條對應的裝甲板的角點
    Point left_up,left_down,right_up,right_down;
    int color_flag = blue; //目测为红色
    while(1)
    { 
        
        if(!cap.read(src)) break;
        // 重建全黑图像
        Mat black(src.size(),CV_8UC1,Scalar(0,0,0));
        // 首先提取到了某一帧图片
        split(src, channels);
        if(color_flag == red)
        {
            midImage = channels.at(2) - channels.at(0);
        }else
        {
            midImage = channels.at(0) - channels.at(2);
        }
        // 阈值分割，使得得到较好结果的方法
        double th = threshold(midImage, binImage, 120, 255, THRESH_BINARY|THRESH_OTSU);
        if(th-10 > 0)
        {
            threshold(midImage,binImage, th-10, 255, THRESH_BINARY);
        }
        //  找轮廓
        findContours(binImage,contours,hierarchy,RETR_CCOMP,CHAIN_APPROX_NONE);
        for(int i = 0; i < contours.size(); i++)
        {
            RotatedRect rotate_Rect;
            Rect rect;
            rect = boundingRect(Mat(contours[i]));               //拟合所有轮廓的直立矩形
            rotate_Rect = minAreaRect(contours[i]);             //拟合所有轮廓的旋转矩形
            float contour_area = contourArea(contours[i]);
            if(contour_area < 100 || contour_area > 30000) continue;//轮廓面积筛选
            float width = rotate_Rect.size.width;                 //旋转矩阵的宽度
            float height = rotate_Rect.size.height;               //旋转矩阵的高度
            float rate_w_h;
            if(width > height)
            {
                rate_w_h = width/height;
            }
            else
            {
                rate_w_h = height/width;
            }
            if(rate_w_h < 1 || rate_w_h > 7) continue;             //旋转矩形长宽比筛选
            // debug用
            // drawContours(, contours, i, Scalar(0,255,0));
            // drawRect(, rotate_Rect);
            // drawRect(,rotate_Rect);
            // cout<<  <<endl;
            contours_.push_back(contours[i]);
            rotate_Rect_Group.push_back(rotate_Rect);
            rect_Group.push_back(rect);
        }
        // debug用
        // cout<<contours_.size()<<endl;
        if(contours_.size() < 20) 
        {
            //筛选出来的灯条进行左右配对
            for(int i = 0; i < contours_.size(); i++)
            {
                for(int j = i+1; j < contours_.size(); j++)
                {
                    // debug用 cout<<abs(rotate_Rect_Group[i].angle - rotate_Rect_Group[j].angle)<<endl;
                    // 得到兩個旋轉矩形的中心點
                    center1 = getRotateRectCenter(rotate_Rect_Group[i]);
                    center2 = getRotateRectCenter(rotate_Rect_Group[j]);
                    if(abs(center1.y - center2.y) > 20) continue;
                    if(abs(center1.x - center2.x) < 50 || abs(center1.x - center2.x) > 100) continue;
                    if(abs(rotate_Rect_Group[i].angle - rotate_Rect_Group[j].angle) < 8 ||
                       abs(rotate_Rect_Group[i].angle - rotate_Rect_Group[j].angle) > 82)
                    {
                        //假设配对成功
                        // Mat black(src.size(),CV_8UC1,Scalar(0,0,0));
                        interstPoints = detectpoints(src, rotate_Rect_Group[i], rotate_Rect_Group[j]);
                        left_up = interstPoints[0];
                        left_down = interstPoints[3];
                        right_up = interstPoints[1];
                        right_down = interstPoints[2];
                        circle(src, right_down, 5, Scalar(255,255,255));
                        circle(src, left_down, 5, Scalar(255,255,255));
                        circle(src, right_up, 5, Scalar(255,255,255));
                        circle(src, left_up, 5, Scalar(255,255,255));
                        line(src, left_down, right_up, Scalar(255,255,255), 2);
                        line(src, left_up, right_down, Scalar(255,255,255), 2);
                        Point center = Point((left_up.x + right_down.x)/2,(left_up.y + right_down.y)/2);
                        circle(src, center, 5, Scalar(255,255,255));
                        // debug用
                        // drawRect(src,rotate_Rect_Group[i]);
                        // drawRect(src,rotate_Rect_Group[j]);
                        // circle(src, center1, 5, Scalar(255), 5, -1);
                        // circle(src, center2, 5, Scalar(255), 5, -1);
                    }

                }
            }
        }

    
    // circle(src, right_down, 5, Scalar(255,255,255));
    // circle(src, left_down, 5, Scalar(255,255,255));
    // circle(src, right_up, 5, Scalar(255,255,255));
    // circle(src, left_up, 5, Scalar(255,255,255));
    // line(src, left_down, right_up, Scalar(255,255,255), 2);
    // line(src, left_up, right_down, Scalar(255,255,255), 2);
    // Point center = Point((left_up.x + right_down.x)/2,(left_up.y + right_down.y)/2);
    // circle(src, center, 5, Scalar(255,255,255));  
    imshow("source",src);
    waitKey(10);
    // 防止超过最大进程内存
    contours.clear();
    contours_.clear();                     
    rotate_Rect_Group.clear();
    rect_Group.clear();
    }
    cap.release();
    return 0;
}